/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvWorld/Src/line_path.cpp
 * @Description  : 实现了LinePath类，表示一条线性路径。
 *                 它包含构造函数、初始化函数、获取路径朝向角度的函数、创建轨迹对象的函数以及创建LinePath对象的函数。 
 *                 在初始化过程中，计算路径的方向角和长度，并根据移动方向调整方向角。 
 *                 该类的主要功能是为移动对象提供路径信息和轨迹生成。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:36:56
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#include <stdio.h>
#include "path.h"
#include "tools.h" 

//////////////////////////////////////////////////////////////////////////////
//   Implementation of class "LinePath".

//
//   CLinePath: The constructor.
//
LinePath::LinePath(USHORT id, USHORT start_node, USHORT end_node,
                   float velocity_limit, USHORT guide_type, MoveDirTag move_dir)
    : Path(id, start_node, end_node, velocity_limit, move_dir, PathType::LINE_TYPE, guide_type) 
{
    Init();
}

/**
 * @brief 初始化 LinePath 对象
 *
 * 初始化 LinePath 对象，设置路径的方向角和长度，并根据移动方向调整方向角。
 */
void LinePath::Init() 
{
    Point start = GetStartPoint();
    Point end = GetEndPoint();
    Line line(start, end);

    heading_angle_ = line.SlantAngle();
    size_ = line.Length();

    if (move_dir_ == MoveDirTag::Backward) {
        heading_angle_ = !heading_angle_;
    }
}

/**
 * @brief 获取当前路径的朝向角度
 *
 * 获取当前路径的朝向角度，该角度保存在成员变量 heading_angle_ 中。
 *
 * @param node 节点对象，此参数在此函数实现中未使用
 * @return 返回类型为 Angle 的引用，指向当前路径的朝向角度
 */
Angle& LinePath::GetHeading(Node node/*node*/) 
{
    return heading_angle_;
}

/**
 * @brief 创建轨迹对象
 *
 * 创建一个 LineTrajectory 对象，并基于当前 LinePath 的起点和终点，以及移动方向，生成对应的轨迹。
 *
 * @return 返回生成的 LineTrajectory 对象指针
 */
Trajectory* LinePath::MakeTraj() 
{
    auto* line_traj = new LineTrajectory;
    Point start = GetStartPoint();
    Point end = GetEndPoint();
    line_traj->CreateTrajectory(start, end, move_dir_);
    return line_traj;
}

/**
 * @brief 创建 LinePath 对象
 *
 * 该函数用于初始化 LinePath 对象，并返回一个布尔值表示是否创建成功。
 *
 * @return 布尔值，如果创建成功则返回 true，否则返回 false。
 */
bool LinePath::Create() 
{
    Init();
    return true;
}

